#include "Server.h"
#include "base64.h"
#include "process.h"
#include <arpa/inet.h>
#include <netinet/in.h>
#include <stdio.h>
#include <stdlib.h>
#include <sys/socket.h>
#include <unistd.h>
#include <vector>
#include <pthread.h>
#include <algorithm>
#include <map>
#include "CommandParameters.h"
#include <cmath>
#include "Processtree.h"

void releasePort(int port)
{
  std::string command = "fuser -k " + std::to_string(port) + "/tcp";
  int result = std::system(command.c_str());

  if (result == 0)
  {
    std::cout << "Port " << port << " released successfully." << std::endl;
  }
  else
  {
    std::cerr << "Failed to release port " << port << "." << std::endl;
  }
}

bool isRobotAtTargetPoint(double x, double y)
{
  if ((std::abs(getRobot_x() - x) + std::abs(getRobot_y() - y)) < 3)
  {
    return false;
  }
  return true;
}

void *working(void *args)
{
  do
  {
    if (getRobotGrasping())
    {
      robot_target_x = Affine[0][0] * camera_target_x + Affine[0][1] * camera_target_y + Affine[0][2];
      robot_target_y = Affine[1][0] * camera_target_x + Affine[1][1] * camera_target_y + Affine[1][2];
      getRobotCalibration();
      std::vector<cv::Point2f> robotCalib = getRobotCalibrations();
      if (!robotCalib.empty())
      {
        std::vector<cv::Point2f>::const_iterator it = robotCalib.begin();
        RobotMotion(it->x, it->y, Controller_fd);
        sleep(1);
        RobotMotion(robot_target_x, robot_target_y, Controller_fd);
      }
      RobotCalibrations.clear();
      while (isRobotAtTargetPoint(robot_target_x, robot_target_y))
      {
        if (!getRobotGrasping())
        {
          break;
        }
      }
      sleep(1);
    }

  } while (true);
  return NULL;
}

// void *working(void *args)
// {
//   do
//   {
//     robot_target_x = Affine[0][0] * camera_target_x + Affine[0][1] * camera_target_y + Affine[0][2];
//     robot_target_y = Affine[1][0] * camera_target_x + Affine[1][1] * camera_target_y + Affine[1][2];
//     if (getRobotGrasping())
//     {
//       getRobotCalibration();
//       std::vector<cv::Point2f> robotCalib = getRobotCalibrations();
//       if (!robotCalib.empty())
//       {
//         for (std::vector<cv::Point2f>::const_iterator it = robotCalib.begin(); it != robotCalib.end(); ++it)
//         {
//           RobotMotion(it->x, it->y, Controller_fd);
//           RobotMotion(robot_target_x, robot_target_y, Controller_fd);
//         }
//       }
//       RobotCalibrations.clear();
//       sleep(5);
//     }
//   } while (true);
//   return NULL;
// }

void *Socket7000(void *args)
{
  do
  {
    Controller_fd = socket(AF_INET, SOCK_STREAM, 0);
    if (Controller_fd == -1)
    {
      std::cerr << "socket 7000 failed:" << std::endl;
    }

    // 2. 连接服务器
    struct sockaddr_in addr;
    addr.sin_family = AF_INET;
    addr.sin_port = htons(Controller_Port); // 大端端口
    inet_pton(AF_INET, IP.c_str(), &addr.sin_addr.s_addr);

    int ret = connect(Controller_fd, (struct sockaddr *)&addr, sizeof(addr));
    if (ret == -1)
    {
      std::cerr << "Controller socket connect " << Controller_Port << " failed:" << std::endl;
      sleep(3);
      continue;
    }
    std::cout << "Controller socket connect " << Controller_Port << " successfully !" << std::endl;
    logMessage("Controller socket successfully connected !");

    pthread_t tid;
    pthread_create(&tid, NULL, working, NULL);
    pthread_detach(tid);

    // 3. 和服务器端通信
    while (true)
    {

      char buf[2048];

      // 接收控制器端口数据
      memset(buf, 0, sizeof(buf));
      int len = read(Controller_fd, buf, sizeof(buf));

      if (len > 0)
      {
        // 将接收到的数据添加到ReceivedDataBufferController
        ReceivedDataBufferController.insert(ReceivedDataBufferController.end(), buf, buf + len);
        std::vector<Message> messages = DecodeMessage(ReceivedDataBufferController, Controller_Byte_Length);
        for (auto message : messages)
        {
          if (message.command == Robot_Position_Return)
          {
            Json::Reader reader; // 一个 json读取类
            Json::Value root;    // 一个json数据类
            reader.parse(message.data, root);

            if (root["replyData"]["realPosMCS"].isArray())
            {
              // 如果 realPosMCS 是一个数组，将其转换为 std::vector<double>
              std::vector<double> coord;
              for (const auto &value : root["replyData"]["realPosMCS"])
              {
                coord.push_back(value.asDouble());
              }
              if (coord.size() == 6)
              {
                Json::Value robot_root;
                Json::FastWriter robot_writer;

                robot_root["RobotX"] = coord[0];
                robot_root["RobotY"] = coord[1];
                robot_root["RobotZ"] = coord[2];
                robot_root["RobotA"] = coord[3];
                robot_root["RobotB"] = coord[4];
                robot_root["RobotC"] = coord[5];
                updateRobot_xy(coord[0], coord[1]);
                std::vector<uint8_t> encodedData = EncodeMessage(ROBOT_RETURN_CORD, robot_writer.write(robot_root));
                send(Upper_Computer_fd, encodedData.data(), encodedData.size(), 0);
                logMessage("Send Corrd! ");
              }
            }
            else
            {
              std::cout << "realPosMCS is not an array!" << std::endl;
            }
          }
        }
      }
      else if (len == 0)
      {
        printf("Controller disconnected...\n");
        break;
      }
      else
      {
        perror("read");
        break;
      }
    }
    close(Controller_fd);
  } while (true);

  return 0;
}

void Socket10000()
{
  int res;
  do
  {
    // 创建Socket
    int socket_fd = socket(AF_INET, SOCK_STREAM, 0);
    if (socket_fd == -1)
    {
      logMessage("socket failed: ");
      exit(0);
    }

    // 1.准备通讯地址（必须是服务器的）
    struct sockaddr_in addr;
    addr.sin_family = AF_INET;
    // 将一个无符号短整型的主机数值转换为网络字节顺序，即大尾顺序(big-endian)
    addr.sin_port = htons(Upper_Computer_Port);
    // addr.sin_addr.s_addr = inet_addr("192.168.2.101"); //
    // net_addr方法可以转化字符串，主要用来将一个十进制的数转化为二进制的数，用途多于ipv4的IP转化。
    addr.sin_addr.s_addr = INADDR_ANY;
    // inet_pton(AF_INET, "10.168.3.108", &addr.sin_addr.s_addr);

    // 2.bind()绑定
    // 参数一：0的返回值（socket_fd）
    // 参数二：(struct sockaddr*)&addr 前面结构体，即地址
    // 参数三: addr结构体的长度

    do
    {
      res = bind(socket_fd, (struct sockaddr *)&addr, sizeof(addr));
      if (res == -1)
      {
        logMessage("bind failed: ");
        releasePort(Upper_Computer_Port);
        sleep(5);
      }
      else
      {
        logMessage("bind: " + std::to_string(Upper_Computer_Port) + " success");
        std::cout << "bind: " << Upper_Computer_Port << " success" << std::endl;
      }
    } while (res == -1);

    // 3.监听客户端listen()函数
    // 参数二：进程上限，一般小于30
    listen(socket_fd, 128);
    // 4. 阻塞等待并接受客户端连接
    struct sockaddr_in cliaddr;
    socklen_t clilen = sizeof(cliaddr);
    do
    {
      Upper_Computer_fd = accept(socket_fd, (struct sockaddr *)&cliaddr, &clilen);
      if (Upper_Computer_fd == -1)
      {
        logMessage("accept failed: ");
      }
    } while (Upper_Computer_fd == -1);

    // 打印客户端的地址信息
    char ip[24] = {0};
    printf("客户端的IP地址: %s, 端口: %d\n",
           inet_ntop(AF_INET, &cliaddr.sin_addr.s_addr, ip, sizeof(ip)),
           ntohs(cliaddr.sin_port));

    std::string JsonString1 = "已连接";
    std::vector<uint8_t> encodedData1 = EncodeMessage(0x10a1, JsonString1);
    send(Upper_Computer_fd, encodedData1.data(), encodedData1.size(), 0);

    std::vector<uint8_t> EncodedData;
    bool SubcontractingFlag = false;
    int dataLength = 0;
    while (true)
    {
      char buffer[2048];
      memset(buffer, 0, sizeof(buffer));
      ssize_t bytesReceived;
      bytesReceived = read(Upper_Computer_fd, buffer, sizeof(buffer));

      if (bytesReceived > 0)
      {

        // 将接收到的数据添加到ReceivedDataBufferUpper
        ReceivedDataBufferUpper.insert(ReceivedDataBufferUpper.end(), buffer, buffer + bytesReceived);

        std::vector<Message> messages = DecodeMessage(ReceivedDataBufferUpper);
        for (auto message : messages)
        {
          MessageProcessor(message);
        }
      }
      else if (bytesReceived == 0)
      {
        logMessage("Client disconnected...");
        break;
      }
      else
      {
        logMessage("Client read...");
        perror("read");
        break;
      }
    }

    IsServerStart = false;
    close(Upper_Computer_fd);
    close(socket_fd);
  } while (true);
}

void *SocketServer_TeachingDevice(void *args)
{
  do
  {
    bool *logExecuted = new bool(false); // 使用动态内存分配创建一个 bool 类型的指针，并初始化为 false
    int socket_fd;
    int res;
    do
    {
      // 创建Socket
      socket_fd = socket(AF_INET, SOCK_STREAM, 0);

      if (socket_fd == -1 && !(*logExecuted))
      {
        logMessage("Failed to create socket communication between vision and teaching pendant!");
        *logExecuted = true; // 将指针所指向的值设置为 true，表示已经执行过 logMessage
      }
    } while (socket_fd == -1);
    logMessage("Successfully created socket communication between vision and teaching pendant!");

    // 循环结束后释放内存
    delete logExecuted;

    // 1.准备通讯地址（必须是服务器的）
    struct sockaddr_in addr;
    addr.sin_family = AF_INET;
    // 将一个无符号短整型的主机数值转换为网络字节顺序，即大尾顺序(big-endian)
    addr.sin_port = htons(Communication_Port);
    // addr.sin_addr.s_addr = inet_addr("192.168.2.101"); //
    // net_addr方法可以转化字符串，主要用来将一个十进制的数转化为二进制的数，用途多于ipv4的IP转化。
    addr.sin_addr.s_addr = INADDR_ANY;
    // inet_pton(AF_INET, "10.168.3.108", &addr.sin_addr.s_addr);

    do
    {
      res = bind(socket_fd, (struct sockaddr *)&addr, sizeof(addr));
      if (res == -1)
      {
        logMessage("bind " + std::to_string(Communication_Port) + " failed!");
        releasePort(Communication_Port);
        sleep(5);
      }
      else
      {
        logMessage("bind: " + std::to_string(Communication_Port) + " success");
        std::cout << "bind: " << Communication_Port << " success" << std::endl;
      }
    } while (res == -1);

    // 3.监听客户端listen()函数
    // 参数二：进程上限，一般小于30
    listen(socket_fd, 128);
    // 4. 阻塞等待并接受客户端连接
    struct sockaddr_in cliaddr;
    socklen_t clilen = sizeof(cliaddr);
    do
    {
      Communication_fd = accept(socket_fd, (struct sockaddr *)&cliaddr, &clilen);
      if (Communication_fd == -1)
      {
        logMessage(std::to_string(Communication_fd) + " port accept failed!");
      }
    } while (Communication_fd == -1);

    // 打印客户端的地址信息
    char ip[24] = {0};
    printf("客户端的IP地址: %s, 端口: %d\n",
           inet_ntop(AF_INET, &cliaddr.sin_addr.s_addr, ip, sizeof(ip)),
           ntohs(cliaddr.sin_port));

    while (true)
    {
      char buffer[2048];
      memset(buffer, 0, sizeof(buffer));
      ssize_t bytesReceived;
      bytesReceived = read(Communication_fd, buffer, sizeof(buffer));

      if (bytesReceived > 0)
      {
        std::string receivedString(buffer);
        std::cout << receivedString << "  " << (receivedString == "TRG") << std::endl;
        if (receivedString == "TRG")
        {
          std::string jsonFilePath = executablePath + "/Param/processtree.json";
          ProcessTree processTree(jsonFilePath);
          processTree.execute();
        }
      }
      else if (bytesReceived == 0)
      {
        logMessage("Teaching Pendant Client disconnected...");
        break;
      }
      else
      {
        logMessage("Teaching Pendant Client read...");
        perror("read");
        break;
      }
    }
    close(Communication_fd);
    close(socket_fd);
  } while (true);
}
